package indoor.nexd.com.nexdmap.routeplan;
/*
 *  Author: Xiaolong_Shen @ Nexd_Tech
 *  Reference: ALGOLIST
 *  Briefs:
 *  	Realization of Dijkstra Algorithm for shortest path search
 *  	@Vertex is like the link map
 * 		@Edge store the connection relationship
 * 		@Dijkstra is the main class for getting the path computation and the path creation
 * 
 * 	Efficiency Update:
 * 		(i) Speed with Estimated edges and vertexes:
 * 				100 Vertices with 10 connection average: 3 millisecond. [Done]
 * 		(ii) Loading Speed with many edges and vertexes:
 * 				Liang Yuan should give a sample json format road map.
 */

//说明：先new一个Dijkstra对象，然后load_config，之后就可以按需要调用其他方法了。如果没有load_config，程序会报错。
//示例：
//public class DijkstraTest {
//    public void test(){
//        String file_path = this.getClass().getResource("/RDCampus.json").getPath();
//        Dijkstra dj = new Dijkstra();
//        try {
//            dj.load_config(file_path);
//        } catch (IOException e) {
//            e.printStackTrace();
//        }
//        long source_ind = 1010805400070004L;
//        double p[]={15,20};
//        long floorId=101080540005L;
//        long target_ind = 1010805400080095L;
//        int m=2;
//        //m=0: none is preferred; m=1: elevators are preferred;
//        //m=2: escalators are preferred, m=3: stairs are preferred.
//        String res_json = dj.get_path_poi(source_ind, target_ind, floorId, 1);
//        String res_json = dj.get_path_poi(p, target_ind, 1);
//        int threshold=5;
//        List<Long> poiList=dj.searchPoi(p,floorId,threshold);
//    }
//}

//输出json说明：
//        {
//        //101080540007: 后面[]中的节点的楼层ID; 0: 到达[]中节点需要乘坐电梯的次数。
//        "1010805400070":[
//        {
//        "Name":"1010805400070004",//节点名字
//        "poiLabel":"0", //节点标签。0对应非梯节点，1对应竖梯电梯口，2对应扶梯电梯口，3对应消防楼梯口
//        "Location":["20.7745968398","17.2145381875"],//该节点坐标
//        "Distance":"0.0"//该节点距离上一个节点的距离。如果为1000，表示从上一个节点到本节点的路径为梯。
//        }
//        ]
//        }
//注意：坐标到POI中，如果需要新添加节点，节点名字为floorId加上“++++”。

import java.io.IOException;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;

public class Dijkstra {
    /*
     *  Private Variables should be loaded in by function _load_in(file_path)
	 */

    Map<String, Integer> name_poiLabel = new HashMap<String, Integer>();
    int switchmark = 0;
    private List<Vertex> map_vertices = new ArrayList<Vertex>();
    private List<Vertex> vList = new ArrayList<Vertex>();
    private boolean Load_Flag = false;
    private List<String> reference = new ArrayList<String>();
    private List<String> sList = new ArrayList<String>();
    private double d = 0;


    public Dijkstra() {
    }

    public Dijkstra(List<Vertex> map_vertices, List<String> reference, Map<String, Integer> name_poiLabel) {
        this.vList = map_vertices;
        this.sList = reference;
        this.name_poiLabel = name_poiLabel;
        Load_Flag = true;
    }

    public void load_config(String map_path) throws IOException {
        Dijkstra_Json d_json = new Dijkstra_Json();
        d_json._load_config(map_path);
        vList = d_json.map_vertices;
        sList = d_json.reference;
        name_poiLabel = d_json.name_poiLabel;
        Load_Flag = true;
    }

    public void load_config(Dijkstra_Json d_json) throws IOException {
        vList = d_json.map_vertices;
        sList = d_json.reference;
        Load_Flag = true;
    }

    public boolean checkConnectivity() {
        for (Vertex v : map_vertices) {
            computePaths(v);
            for (Vertex u : map_vertices) {
                if (u.minDistance == Double.POSITIVE_INFINITY) {
                    System.out.println(v.name + " and " + u.name + " are not connected.");
                    for (Vertex vt : map_vertices) {
                        vt.minDistance = Double.POSITIVE_INFINITY;
                    }
                    return false;
                }
                u.minDistance = Double.POSITIVE_INFINITY;
            }
        }
        return true;
    }



    public List<Vertex> get_path_poi_poi(long source_ind, long target_ind) {

        map_vertices = new ArrayList<Vertex>(vList);
        reference = new ArrayList<String>(sList);

        int source = this.reference.indexOf(Long.toString(source_ind));
        int target = this.reference.indexOf(Long.toString(target_ind));
//    	System.out.println("src:" + source + "; " + target);

        if (source == -1 || target == -1) {
            System.out.println("Wrong!");
            return null;
        } else {

            List<Vertex> res_path = _get_path(source, target);
            for (Vertex v : map_vertices) {
                v.previous = null;
                v.minDistance = Double.POSITIVE_INFINITY;
            }
            return res_path;
//            List<Vertex> res_path = _get_path(source, target);
//            for (Vertex v : map_vertices) {
//                v.previous = null;
//                v.minDistance = Double.POSITIVE_INFINITY;
//            }
//            return _get_path(source, target);
        }
    }


    public String get_path_poi(long source_ind, long target_ind) {

        map_vertices = new ArrayList<Vertex>(vList);
        reference = new ArrayList<String>(sList);

        int source = this.reference.indexOf(Long.toString(source_ind));
        int target = this.reference.indexOf(Long.toString(target_ind));
//    	System.out.println("src:" + source + "; " + target);

        if (source == -1 || target == -1) {
            System.out.println("Wrong!");
            return "";
        } else {
            List<Vertex> res_path = _get_path(source, target);
//            System.out.println("PATH: "+res_path);
            String res_json = _to_json(res_path);
            for (Vertex v : map_vertices) {
                v.previous = null;
                v.minDistance = Double.POSITIVE_INFINITY;
            }
            return res_json;
        }
    }

    public String get_path_poi(long source_ind, long target_ind, int m) {

        map_vertices = new ArrayList<Vertex>(vList);
        reference = new ArrayList<String>(sList);

        int source = this.reference.indexOf(Long.toString(source_ind));
        int target = this.reference.indexOf(Long.toString(target_ind));
//    	System.out.println("src:" + source + "; " + target);

        if (source == -1 || target == -1) {
            System.out.println("Wrong!");
            return "";
        } else {
            List<Vertex> res_path = _get_path(source, target, m);
//            System.out.println("PATH: "+res_path);
            String res_json = _to_json(res_path);
            for (Vertex v : map_vertices) {
                v.previous = null;
                v.minDistance = Double.POSITIVE_INFINITY;
            }
//            System.out.println("++++++++++++++++++++");
            return res_json;
        }
    }

    /*
     * computePaths and getShortestPathTo is the core function to calculate results
     */
    private void computePaths(Vertex source) {
        //   source.minDistance = 0.;
        source.minDistance = d;
        PriorityQueue<Vertex> vertexQueue = new PriorityQueue<Vertex>();
        vertexQueue.add(source);

        while (!vertexQueue.isEmpty()) {
            Vertex u = vertexQueue.poll();
            // Visit each edge exiting u
            for (Edge e : u.adjacencies) {
                Vertex v = e.target;
                double weight = e.weight;
                double distanceThroughU = u.minDistance + weight;
                if (distanceThroughU < v.minDistance) {
                    vertexQueue.remove(v);
                    v.minDistance = distanceThroughU;
                    v.previous = u;
                    vertexQueue.add(v);
                }
            }
        }
    }

    private void computePaths(Vertex source, int m) {
        source.minDistance = d;
        PriorityQueue<Vertex> vertexQueue = new PriorityQueue<Vertex>();
        vertexQueue.add(source);

        while (!vertexQueue.isEmpty()) {
            Vertex u = vertexQueue.poll();

            // Visit each edge exiting u
            for (Edge e : u.adjacencies) {
                Vertex v = e.target;
                double weight = e.weight;

                if (m != 0 && m == e.label) {
                    weight = weight / 10.0;
                }

                double distanceThroughU = u.minDistance + weight;
                if (distanceThroughU < v.minDistance) {
                    vertexQueue.remove(v);
                    v.minDistance = distanceThroughU;
                    v.previous = u;
                    vertexQueue.add(v);
                }
            }
        }
    }

    // private funciton for get path. Intergrated compute and get path.

    private List<Vertex> getShortestPathTo(Vertex target) {
        List<Vertex> path = new ArrayList<Vertex>();
        for (Vertex vertex = target; vertex != null; vertex = vertex.previous) {
            path.add(vertex);
        }
        Collections.reverse(path);
        return path;
    }

    private List<Vertex> _get_path(int source_ind, int target_ind) {
        if (Load_Flag) {
            computePaths(map_vertices.get(source_ind));
            return getShortestPathTo(map_vertices.get(target_ind));
        } else {
            return null;
        }
    }

    private List<Vertex> _get_path(int source_ind, int target_ind, int m) {
        if (Load_Flag) {
            computePaths(map_vertices.get(source_ind), m);
            return getShortestPathTo(map_vertices.get(target_ind));
        } else {
            return null;
        }
    }

    public List<Vertex> get_path_poi(double[] p, long target_ind, long floorId) {

        map_vertices = new ArrayList<Vertex>(vList);
        reference = new ArrayList<String>(sList);

        Vertex newVertex = get_nearest(p, floorId);//获得最近POI（新的POI或者已有的POI）
//    	System.out.println("The new POI: "+newVertex.name+" "+newVertex.location[0]+" "+newVertex.location[1]);
        if (!reference.contains(newVertex.name)) {
            map_vertices.add(newVertex);
            reference.add(newVertex.name);
        }
        int source = this.reference.indexOf(newVertex.name);
        int target = this.reference.indexOf(Long.toString(target_ind));
//        System.out.println("src:" + source + "; "+ target);
        if (source == -1 || target == -1) {
            System.out.println("Road information is wrong or missing!");
            List<Vertex> res_path = new ArrayList<>();
            return res_path;
        } else {
            List<Vertex> res_path = _get_path(source, target);
            for (Vertex v : map_vertices) {
                v.previous = null;
                v.minDistance = Double.POSITIVE_INFINITY;
            }
            return res_path;
        }
    }

    /**
     * @param p
     * @param target_ind
     * @param floorId
     * @param m          0:无优先 1:直梯 2:扶梯 3:楼梯
     * @return
     */
    public String get_path_poi(double[] p, long target_ind, long floorId, int m) {
//        long st = System.nanoTime();
        map_vertices = new ArrayList<Vertex>(vList);
        reference = new ArrayList<String>(sList);
//        long ed = System.nanoTime();
//       System.out.println("Diff Time" + (ed-st));

        Vertex newVertex = get_nearest(p, floorId);//获得最近POI（新的POI或者已有的POI）
//    	System.out.println("The new POI: "+newVertex.name+" "+newVertex.location[0]+" "+newVertex.location[1]);
        if (!reference.contains(newVertex.name)) {
            map_vertices.add(newVertex);
            reference.add(newVertex.name);
        }
        int source = this.reference.indexOf(newVertex.name);
        int target = this.reference.indexOf(Long.toString(target_ind));
//        System.out.println("src:" + source + "; "+ target);
        if (source == -1 || target == -1) {
            return "";
        } else {
            List<Vertex> res_path = _get_path(source, target, m);
            String res_json = _to_json(res_path);
            for (Vertex v : map_vertices) {
                v.previous = null;
                v.minDistance = Double.POSITIVE_INFINITY;
            }
            return res_json;
        }
    }

    /**
     * 说明
     * 该算法计算坐标到POI的路径规划，输入为坐标及其所在楼层（用字母表示，v或q），输出为Vertex的序列。
     * 该算法考虑以下情况：
     * 如果不能朝垂直于路的方向走到路上，则最近的POI为路径规划的起点，newVertex为最近的POI；
     * 如果可以朝垂直于路的方向走到最近的路上，则新增POI，新增的POI的名字为坐标所在楼层的符号floor+"++++"，如101080540006++++。
     *
     * @param p
     * @param floor
     * @return
     */

    private Vertex get_nearest(double[] p, String floor) {

//        map_vertices = new ArrayList<>(vList);
//        reference = new ArrayList<>(sList);
        double nearest = Double.POSITIVE_INFINITY;
        Vertex newVertex = new Vertex(floor + "++++");
        String[] source_name = {"", ""};

        List<Vertex> vList = new ArrayList<Vertex>();//Choose the vertexes on the appointed floor
        for (Vertex v : map_vertices) {
            if (v.name.substring(0, 12).equals(floor)) {
                vList.add(v);
            }
        }

        for (Vertex v : vList) {
            for (Edge e : v.adjacencies) {
                Vertex u = e.target;
                double[] o = point_to_line(v.location, u.location, p);
                double temp_distance = distance(o, p);
                double[] v_o = {o[0] - v.location[0], o[1] - v.location[1]};
                double[] o_u = {u.location[0] - o[0], u.location[1] - o[1]};
                //点到线段的最短距离，所以要判断垂足是不是在线段上
                if (innerproduct(v_o, o_u) < 0) {
//            		System.out.println("****************************************************");
                    temp_distance = distance(v.location, p) < distance(u.location, p) ? distance(v.location, p) : distance(u.location, p);
                }
                if (!u.name.substring(0, 12).equals(floor)) {
                    temp_distance = 2000.0;//给楼梯赋值一个足够大的值，以避险朝垂直于楼梯的方向走到楼梯上。
//        			System.out.println("*******************************************************");
                }
                if (temp_distance < nearest) {
                    nearest = temp_distance;
                    source_name[0] = v.name;
                    source_name[1] = u.name;
                }
            }
        }
        Vertex v1 = map_vertices.get(reference.indexOf(source_name[0]));
        Vertex v2 = map_vertices.get(reference.indexOf(source_name[1]));
        double[] o = point_to_line(v1.location, v2.location, p);
        double[] v1_o = {o[0] - v1.location[0], o[1] - v1.location[1]};
        double[] o_v2 = {v2.location[0] - o[0], v2.location[1] - o[1]};
        double[] v1_v2 = {v1_o[0] + o_v2[0], v1_o[1] + o_v2[1]};
        //If the target vertex is on another floor, then p is near an escalator or an elevator.
        if (!v2.name.substring(0, 12).equals(floor)) {
            d = distance(p, v1.location);
            return v1;
        } else if (innerproduct(v1_o, v1_v2) < 0) {
            d = distance(p, v1.location);
            return v1;
        } else if (innerproduct(v1_v2, o_v2) < 0) {
            d = distance(p, v2.location);
            return v2;
        } else {
            double[] lo = point_to_line(v1.location, v2.location, p);
            Edge e1 = new Edge(v1, distance(v1.location, lo), 0);
            Edge e2 = new Edge(v2, distance(v2.location, lo), 0);
            d = distance(p, lo);
            newVertex.adjacencies = new ArrayList<Edge>();
            newVertex.adjacencies.add(e1);
            newVertex.adjacencies.add(e2);
            newVertex.location = lo;
            newVertex.minDistance = d;
            name_poiLabel.put(newVertex.name, 0);
            return newVertex;
        }
    }

    private double[] point_to_line(double[] a, double[] b, double[] x) {
        double[] ba = {a[0] - b[0], a[1] - b[1]};
        double[] bx = {x[0] - b[0], x[1] - b[1]};
        double inner = innerproduct(ba, bx);
        double d = distance(a, b);
        if (Math.abs(d) < 0.01) {
            return a;
        }
        double r = inner / (d * d);
        double[] o = {0, 0};
        o[0] = b[0] + ba[0] * r;
        o[1] = b[1] + ba[1] * r;
        return o;
    }

    private double innerproduct(double[] a, double[] b) {
        return a[0] * b[0] + a[1] * b[1];
    }

    private double distance(double[] q, double[] p) {
        double _x = Math.abs(q[0] - p[0]);
        double _y = Math.abs(q[1] - p[1]);
        return Math.sqrt(_x * _x + _y * _y);
    }

    private Vertex get_nearest(double[] p, long floorId) {
        String floor = null;
        floor = Long.toString(floorId);
        return get_nearest(p, floor);
    }

    /**
     * 假设POI的id共16位，前12位为楼层id。
     *
     * @param p
     * @param floorId
     * @param threshold
     * @return
     */
//    public List<Long> searchPoi(double[] p, long floorId, int threshold) {
//
//        map_vertices = new ArrayList<Vertex>(vList);
//        reference = new ArrayList<String>(sList);
//
//        Vertex newVertex=get_nearest(p,floorId);//获得最近POI（新的POI或者已有的POI）
//
////        if(newVertex.previous==null){
////            System.out.println(newVertex.location[1]);
////        }
//
//        if (!reference.contains(newVertex.name)){
//            map_vertices.add(newVertex);
//            reference.add(newVertex.name);
//        }
//        int source = this.reference.indexOf(newVertex.name);
//        computePaths(map_vertices.get(source));
//        List<Long> nearPoi=new ArrayList<Long>();
//        List<Vertex> vetexList=new ArrayList<Vertex>();//Choose the vertexes on the appointed floor
//        for(Vertex v: map_vertices){
//                if (Long.parseLong(v.name.substring(0,12))==floorId){
//                    vetexList.add(v);
////	        		System.out.println(v.name+" Added "+i++);
//                }
//        }
//        for(Vertex v:vetexList){
////        	System.out.println(v.name+": "+v.minDistance);
//            if(Character.isDigit(v.name.charAt(13)) && v.minDistance<threshold){
//                nearPoi.add(Long.parseLong(v.name));
////                System.out.println(v.minDistance);
//            }
//        }
////        System.out.println();
//        for(Vertex v: map_vertices){
//            v.minDistance=Double.POSITIVE_INFINITY;
//        }
//        return nearPoi;
//
//    }

//    public Map<Long,double[]> searchPoi(double[] p, long floorId, int threshold) {
//
//        map_vertices = new ArrayList<Vertex>(vList);
//        reference = new ArrayList<String>(sList);
//
//        Vertex newVertex=get_nearest(p,floorId);//获得最近POI（新的POI或者已有的POI）
//
//
//        if (!reference.contains(newVertex.name)){
//            map_vertices.add(newVertex);
//            reference.add(newVertex.name);
//        }
//        int source = this.reference.indexOf(newVertex.name);
//        computePaths(map_vertices.get(source));
//        Map<Long,double[]> nearPoi=new HashMap<Long,double[]>();
//        List<Vertex> vetexList=new ArrayList<Vertex>();//Choose the vertexes on the appointed floor
//        for(Vertex v: map_vertices){
//            if (Long.parseLong(v.name.substring(0,12))==floorId){
//                vetexList.add(v);
//            }
//        }
//        for(Vertex v:vetexList){
//            if(Character.isDigit(v.name.charAt(13)) && v.minDistance<threshold){
//                nearPoi.put(Long.parseLong(v.name),v.location);
//            }
//        }
//        for(Vertex v: map_vertices){
//            v.minDistance=Double.POSITIVE_INFINITY;
//        }
//        return nearPoi;
//
//    }
    public List<SearchPoiShowBean> searchPoi(double[] p, long floorId, int threshold) {

        map_vertices = new ArrayList<Vertex>(vList);
        reference = new ArrayList<String>(sList);

        List<SearchPoiShowBean> nearPoiBeanList = new ArrayList<SearchPoiShowBean>();

        Vertex newVertex = get_nearest(p, floorId);//获得最近POI（新的POI或者已有的POI）

        if (!reference.contains(newVertex.name)) {
            map_vertices.add(newVertex);
            reference.add(newVertex.name);
        }
        int source = this.reference.indexOf(newVertex.name);
        computePaths(map_vertices.get(source));

//        List<Vertex> vetexList=new ArrayList<Vertex>();//Choose the vertexes on the appointed floor
//        for(Vertex v: map_vertices){
//            if (Long.parseLong(v.name.substring(0,12))==floorId){
//                vetexList.add(v);
////	        		System.out.println(v.name+" Added "+i++);
//            }
//        }

//        List<Vertex> vetexList=new ArrayList<Vertex>();//Choose the vertexes on the appointed building
//        for(Vertex v: map_vertices){
//            if (v.name.substring(0,8).equals((floorId+"").substring(0,8))){
//                vetexList.add(v);
//            }
//        }

        for (Vertex v : map_vertices) {
            double temp = v.minDistance;
            if (Character.isDigit(v.name.charAt(13)) && temp < threshold) {
                System.out.println(temp);
                SearchPoiShowBean nearPoiBean = new SearchPoiShowBean();
                nearPoiBean.setPoiId(v.name);
                nearPoiBean.setDist(temp);
                nearPoiBean.setCurrentX(v.location[0] + "");
                nearPoiBean.setCurrentY(v.location[1] + "");
                nearPoiBeanList.add(nearPoiBean);
            }
        }
        for (Vertex v : map_vertices) {
            v.previous = null;
            v.minDistance = Double.POSITIVE_INFINITY;
        }
        return nearPoiBeanList;

    }

    public List<String> get_reference() {
        return sList;
    }

    public List<Vertex> get_map_vertices() {
        return vList;
    }

    public Map<String, Integer> get_name_poiLabel() {
        return name_poiLabel;
    }

    public String _to_json(List<Vertex> path) {
        String output_json = "{\n";
        int count = 0;
        String last_mark = "";
//		Map<String, String> mark = new HashMap<String, String>();
//		mark.put("v","101001170003");
//		mark.put("q","101001170002");
        last_mark = path.get(0).name.substring(0, 12);
//		System.out.println("LastMark: "+last_mark);
        output_json += "\t\"" + last_mark + switchmark + "\":[\n";
        switchmark++;
        for (Vertex v : path) {
            if (count == 0) {
                if (v.minDistance == Double.POSITIVE_INFINITY) {
                    System.out.println("The target POI CANNOT be reached!");
                    return "";
                }
            }
            output_json += get_block_json(v, last_mark, count);
            last_mark = v.name.substring(0, 12);
//			System.out.println("Last_Mark:" + last_mark);
            count += 1;
//			System.out.println(count+" ______________________________");
        }
        output_json += "\t]\n}\n";
        path.clear();
        switchmark = 0;
        return output_json;
    }

    private String get_block_json(Vertex v, String last_mark, int count) {
        String output_json = "";
        String this_mark = "";
        this_mark = v.name.substring(0, 12);
//		for(String m : mark.keySet()) {
//			if (v.name.contains(m)) {
//				this_mark = m;
//				break;
//			}
//		}
//		System.out.println("This mark: "+ this_mark+ "  Last Mark "+ last_mark);
        if (!last_mark.equals(this_mark)) {
            //Different block
            output_json += "\n\t],\n";
            output_json += "\t\"" + this_mark + switchmark + "\":[\n";
            switchmark++;
            output_json += "\t\t{\n";
            output_json += "\t\t\"Name\":\"" + v.name + "\",\n";
            output_json += "\t\t\"poiLabel\":\"" + name_poiLabel.get(v.name) + "\",\n";
            double[] loc = v.location;
            output_json += "\t\t\"Location\":[\"" + loc[0] + "\",\"" + loc[1] + "\"],\n";
            output_json += "\t\t\"Distance\":\"" + 1000 + "\"\n\t}";
        } else {
            // Same block
//			output_json += "\t"+mark.get(this_mark)+":[\n";
            if (count == 0) {
                output_json += "\t\t{\n";
            } else {
                output_json += ",\n\t\t{\n";
            }
            output_json += "\t\t\"Name\":\"" + v.name + "\",\n";
            output_json += "\t\t\"poiLabel\":\"" + name_poiLabel.get(v.name) + "\",\n";
            double[] loc = v.location;
            output_json += "\t\t\"Location\":[\"" + loc[0] + "\",\"" + loc[1] + "\"],\n";
            if (count == 0) {
                output_json += "\t\t\"Distance\":\"" + v.minDistance + "\"\n\t}";
            } else {
                output_json += "\t\t\"Distance\":\"" + (v.minDistance - v.previous.minDistance) + "\"\n\t}";
            }

        }
        return output_json;
    }
}